#ifndef __SG_PCL_REGION_GROWING_H__
#define __SG_PCL_REGION_GROWING_H__

#include "SGPCLJsonConfig.h"

#include <pcl/segmentation/region_growing.h>
#include <vector>

namespace sgpcl
{

/// <summary>
/// 生长算法参数(基于法向量、曲率和残差？）
/// </summary>
struct SGRegionGrowConfig : public SGPCLJsonConfig
{
  int nNeigbourNum;  // 结构面识别时的邻域数量（用于计算法向和曲率）
  int nMinPntNum;    // 簇的最小合法尺寸（小于该值的簇被当作噪声）
  float fNormalDiff; // 法向量夹角变化阈值（用于生长条件）
  float fCurveDiff;  // 曲率变化阈值（用于生长条件）
  float fResidual;   // 残差阈值（用于生长条件）

  SGPCL_API SGRegionGrowConfig();

  SGPCL_API ~SGRegionGrowConfig() = default;

  /// <summary>
  /// 解析Json字符串，读取参数
  /// </summary>
  /// <param name="sJsonRoot">Json节点</param>
  SGPCL_API void ParseJson(const rapidjson::Value& sJsonRoot) final override;
};

/// <summary>
///
/// </summary>
/// <typeparam name="PointT"></typeparam>
/// <param name="spCloud"></param>
/// <param name="sConfig"></param>
/// <param name="bVis"></param>
/// <param name="spColor"></param>
/// <returns></returns>
template <typename PointT>
std::vector<pcl::PointIndices> RegionGrow(typename pcl::PointCloud<PointT>::Ptr spCloud,
  const SGRegionGrowConfig& sConfig, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& spColor)
{
  //区域生长分割器
  pcl::RegionGrowing<PointT, PointT> reg;

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
  tree->setInputCloud(spCloud);

  //输入分割目标
  reg.setSearchMethod(tree);
  reg.setNumberOfNeighbours(sConfig.nNeigbourNum);
  reg.setInputCloud(spCloud);
  reg.setInputNormals(spCloud);

  if (sConfig.fNormalDiff < 0 && sConfig.fCurveDiff < 0 && sConfig.fResidual < 0)
  {
    SG_PANIC("法向量夹角变化阈值、曲率变化阈值、残差值阈值至少设定一个以进行区域生长");
  }

  //设置限制条件及先验知识
  reg.setMinClusterSize(sConfig.nMinPntNum);
  if (sConfig.fNormalDiff > 0)
    reg.setSmoothnessThreshold(sConfig.fNormalDiff * SGC_DEG_TO_RAD); // 法向量偏转阈值
  if (sConfig.fCurveDiff > 0)
    reg.setCurvatureThreshold(sConfig.fCurveDiff); // 曲率变化阈值
  //if (sConfig.fResidual > 0)
  //  reg.setResidualThreshold(sConfig.fResidual); // 残差阈值

  //一个点云团索引容器，用于存放聚类结果
  std::vector<pcl::PointIndices> vecIndice;
  reg.extract(vecIndice);

  if (vecIndice.empty())
  {
    SG_PANIC("区域生长算法没有分割出任何簇，一般是区域生长算法参数不合适，请调整后重新计算");
  }

  spColor = reg.getColoredCloud();

  return vecIndice;
}

}

#endif // __SG_PCL_REGION_GROWING_H__
